#include "speed_timer.h"

extern volatile ControlMode_t CURR_MODE;
extern volatile float current_target;
volatile float setVelocity = 140;
volatile float velocity;

lowpass_filter_type LPF_Velocity = {0};
PID_LocTypeDef speed_pid;

volatile float setAngle = 2 * _PI;
PID_LocTypeDef pos_pid;

void speed_timer_init(void)
{
  /* enable tmr1 clock */
  crm_periph_clock_enable(CRM_TMR10_PERIPH_CLOCK, TRUE);

  /* tmr1 configuration */
  /* time base configuration */
  /* systemclock/19200/10000 = 1hz */
  tmr_base_init(TMR10, 9, (192000000 / 10000) - 1);
  tmr_cnt_dir_set(TMR10, TMR_COUNT_UP);

  tmr_flag_clear(TMR10, TMR_OVF_FLAG);
  /* overflow interrupt enable */
  tmr_interrupt_enable(TMR10, TMR_OVF_INT, TRUE);
  nvic_irq_enable(TMR1_OVF_TMR10_IRQn, 3, 0);

  speed_pid.kp = 0.06;
  speed_pid.ki = 0.0008;
  speed_pid.kd = 0;
  speed_pid.limit = 0.7;
  speed_pid.ka = 1;
  speed_pid.ns = 0;
  
  pos_pid.kp = 0.5;
  pos_pid.ki = 0.0;
  pos_pid.kd = 0;
  pos_pid.ka = 1;
  pos_pid.limit = 0.7;
  
  LPF_Velocity.sample_freq = 1000;
  LPF_Velocity.bandwidth = 600;
  lowpass_filter_init(&LPF_Velocity);
  
  /* enable tmr10 */
  tmr_counter_enable(TMR10, TRUE);
}

void TMR1_OVF_TMR10_IRQHandler(void)
{
  if(tmr_flag_get(TMR10, TMR_OVF_FLAG) != RESET)
  {
    tmr_flag_clear(TMR10, TMR_OVF_FLAG);
    velocity = lowpass_filtering(&LPF_Velocity,encoder_GetVelocity());
//    //      float velocity = encoder_GetVelocity();
    if (CURR_MODE == VELOCITY)
    {
      current_target = PID_Loc(setVelocity, velocity, &speed_pid);
    }
    else if(CURR_MODE == ANGLE)
    {
      current_target = PID_Loc(setAngle, encoder_GetFullAngle(), &pos_pid);
//      current_target = PID_Loc(setVelocity, velocity, &speed_pid);
    }
  }
}